/*----------------------------------------------------------------------------*/
/* Team 3647
 /* Shooter.java
 /* Has all driving functions and bridge lowering
 /*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;

public class Shooter {

    static final LinearVictor ShooterVictor = new LinearVictor(6);
    private static final Encoder shooterEncoder = new Encoder(1, 2, true, CounterBase.EncodingType.k1X);
    private static final Solenoid HoodHigh = new Solenoid(5);
    private static final Solenoid HoodLow = new Solenoid(6);

    public static void hood() {
        if (DriveBase.Stick1.getRawButton(4)) {
            hoodHigh();
        } else if (DriveBase.Stick1.getRawButton(2)) {
            hoodLow();
        }
    }

    public static void hoodHigh() {
        HoodHigh.set(true);
        HoodLow.set(false);
    }

    public static void hoodLow() {
        HoodHigh.set(false);
        HoodLow.set(true);
    }

    public static void EncoderStart() {
        shooterEncoder.start();
        shooterEncoder.setDistancePerPulse(1.0 / 360.0);
    }

    public static void EncoderReset() {
        shooterEncoder.reset();
    }
    
    private static double prevCounts = 0, prevTime = 0, rate = 0;
    private static final double a = .75;
    private static final int tol = 10; // Tolerance
    public static int getRate() {
        double currCounts = shooterEncoder.get();
        double currTime = Timer.getFPGATimestamp();
        double newRate = Math.abs(currCounts - prevCounts) / Math.abs(currTime - prevTime);
        prevCounts = currCounts;
        prevTime = currTime;
        newRate /= 6.0; //RPM
        rate = rate * a + (1 - a) * newRate;
        rate = ((int) (rate / tol)) * tol;
        return (int)rate;
    }
    
    private static double S = 1, P = .0003, I = .000007,  D = 0.00000001; 
    private static double err = 0, prevErr = 0, prevPrevErr = 0, out = 0;
    public static void PIDv2(int setPoint) {
        prevPrevErr = prevErr;
        prevErr = err;
        err = setPoint - getRate();
        if(Math.abs(err) < 20) {
            err = 0; 
        } 
        if(setPoint < 200) {
            out = 0;
        }
        out += -S * ((P * (err - prevErr)) + (I * err)  + (D * (err - 2 * prevErr + prevPrevErr))); //This is code
        
        if (out > 1) {  //Clamping the output to +- 1
            out = 1;
        }
                
        ShooterVictor.set(((int)(out*100))/100.00);
    }
    
    public static void ClosedLoop() {
        double SpeedPotPosition = PSoC.getAnalog(5);
        if (SpeedPotPosition < 0) {
            SpeedPotPosition = 0;
        }
        SpeedPotPosition = 1 - (SpeedPotPosition / 3.3);
        SpeedPotPosition *= 4200;
        
        if(SpeedPotPosition < 10) {
            SpeedPotPosition = 0;
        }   
        if (PSoC.getDigital(1)) {
            PIDv2(0);
            //System.out.println("\tPOT:\t" + (int)SpeedPotPosition + "\tRate:\t" + getRate() + "\tOut:\t" + out);
            //DashBoard.updateDashboard((int)SpeedPotPosition, rate, out, err);
        } else {
            PIDv2((int)SpeedPotPosition);
            //System.out.println("\tPOT:\t" + (int)SpeedPotPosition + "\tRate:\t" + getRate() + "\tOut:\t" + out);
            //DashBoard.updateDashboard((int)SpeedPotPosition, rate, out, err);
        }
    }
        
    public static void OpenLoop() {
        double SpeedPotPosition = PSoC.getAnalog(5);
        if (SpeedPotPosition < 0) {
            SpeedPotPosition = 0;
        }
        SpeedPotPosition = 1 - (SpeedPotPosition / 3.3);
        SpeedPotPosition *= 2;

        if (SpeedPotPosition < 10) {
            SpeedPotPosition = 0;
        }
        if (PSoC.getDigital(1)) {
            ShooterVictor.set(0);
        } else {
            System.out.println(SpeedPotPosition); 
            ShooterVictor.set(SpeedPotPosition);
        }
    }
}
